function F = calculate_f(q, o_desired, a_desired)
ox_desired = o_desired(1); oy_desired = o_desired(2); oz_desired = o_desired(3);
ax_desired = a_desired(1); ay_desired = a_desired(2); az_desired = a_desired(3);

q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4); q5 = q(5); q6 = q(6);
df_dq5 = ax_desired*(cos(q5)*(cos(q1)*cos(q2)*cos(q4)*sin(q3) - sin(q1)*sin(q4) + cos(q1)*cos(q3)*cos(q4)*sin(q2)) + cos(q2 + q3)*cos(q1)*sin(q5)) + ay_desired*(cos(q5)*(cos(q1)*sin(q4) + cos(q2)*cos(q4)*sin(q1)*sin(q3) + cos(q3)*cos(q4)*sin(q1)*sin(q2)) + cos(q2 + q3)*sin(q1)*sin(q5)) + az_desired*(sin(q2 + q3)*sin(q5) - cos(q2 + q3)*cos(q4)*cos(q5)) - ox_desired*sin(q6)*(sin(q1)*sin(q4)*sin(q5) + cos(q2 + q3)*cos(q1)*cos(q5) - cos(q1)*cos(q2)*cos(q4)*sin(q3)*sin(q5) - cos(q1)*cos(q3)*cos(q4)*sin(q2)*sin(q5)) + oy_desired*sin(q6)*(cos(q1)*sin(q4)*sin(q5) - cos(q2 + q3)*cos(q5)*sin(q1) + cos(q2)*cos(q4)*sin(q1)*sin(q3)*sin(q5) + cos(q3)*cos(q4)*sin(q1)*sin(q2)*sin(q5)) - oz_desired*sin(q6)*(sin(q2 + q3)*cos(q5) + cos(q2 + q3)*cos(q4)*sin(q5));
df_dq6 = ox_desired*(sin(q6)*(cos(q4)*sin(q1) + cos(q1)*cos(q2)*sin(q3)*sin(q4) + cos(q1)*cos(q3)*sin(q2)*sin(q4)) - cos(q6)*(cos(q2 + q3)*cos(q1)*sin(q5) - cos(q5)*sin(q1)*sin(q4) + cos(q1)*cos(q2)*cos(q4)*cos(q5)*sin(q3) + cos(q1)*cos(q3)*cos(q4)*cos(q5)*sin(q2))) + oy_desired*(sin(q6)*(cos(q2)*sin(q1)*sin(q3)*sin(q4) - cos(q1)*cos(q4) + cos(q3)*sin(q1)*sin(q2)*sin(q4)) - cos(q6)*(cos(q2 + q3)*sin(q1)*sin(q5) + cos(q1)*cos(q5)*sin(q4) + cos(q2)*cos(q4)*cos(q5)*sin(q1)*sin(q3) + cos(q3)*cos(q4)*cos(q5)*sin(q1)*sin(q2))) - oz_desired*(cos(q6)*(sin(q2 + q3)*sin(q5) - cos(q2 + q3)*cos(q4)*cos(q5)) + cos(q2 + q3)*sin(q4)*sin(q6));

F = [df_dq5; df_dq6];